#include "PhysicsManager.h"

#include "BaseMoveable.h"


PhysicsManager::PhysicsManager(void)
{
	PhysicsEnts = new EntityList();
}


PhysicsManager::~PhysicsManager(void)
{

}

void PhysicsManager::RegisterPhysicsEntity(BaseEntity* ent)
{
	PhysicsEnts->AddToList(ent);
}

void PhysicsManager::RemoveEntity(BaseEntity* ent)
{
	std::list<BaseEntity*>::iterator i;

	i = PhysicsEnts->mList.begin();
	while (i != PhysicsEnts->mList.end())
	{
		BaseEntity *ent1 = *i;
		if (ent == ent1)
		{
			PhysicsEnts->mList.erase(i);
			return;
		}
		i++;
	}
}

bool PhysicsManager::AABBIntersect(sf::FloatRect &One, sf::FloatRect &Two)
{
	// Compute the intersection boundaries
    float left   = std::max(One.Left,         Two.Left);
    float top    = std::min(One.Top,          Two.Top);
    float right  = std::min(One.Left + One.Width, Two.Left + Two.Width);
    float bottom = std::max(One.Top - One.Height, Two.Top - Two.Height);

    // If the intersection is valid (positive non zero area), then there is an intersection
    if ((left < right) && (top > bottom))
    {
        return true;
    }
    else
    {
        return false;
    }
}

sf::Vector2f PhysicsManager::ProjectShape(PhysicsObject* hull, sf::Vector2f Axis)
{
	float min = Util::VecDot(Axis,hull->ToGlobal(hull->Coords[0]));
	float max = min;
	for (int i = 0; i < hull->GetPointCount(); i++)
	{
		double p = Util::VecDot(Axis, hull->ToGlobal(hull->Coords[i]));
		if (p < min)
		{
			min = p;
		}
		else if (p > max)
		{
			max = p;
		}
	}
	return sf::Vector2f(min,max);
}

float PhysicsManager::ProjectionOverlap(sf::Vector2f One, sf::Vector2f Two)
{
	if (One.y > Two.y)
		return Two.y - One.x;
	else
		return One.y - Two.x;
}

int PhysicsManager::CheckCollision2(PhysicsObject* hull1, PhysicsObject* hull2, CollisionData *CollideInfo)
{
	int iCollideCount = 0;
	float fOverlap = RAND_MAX;
	sf::Vector2f OverAxis;
	sf::Vector2f SmallEdge1;
	sf::Vector2f SmallEdge2;
	PhysicsObject* EdgeOwner;



	if (AABBIntersect(hull1->AABB, hull2->AABB))
	{

		if (hull1->IsCircle() && hull2->IsCircle())
		{
		
		}

		int PointCount1 = hull1->GetPointCount();
		int PointCount2 = hull2->GetPointCount();

		for (int Points1=0; Points1 < PointCount1; Points1++)
		{
			int NextPoint = Points1 + 1;
			if (NextPoint == hull1->GetPointCount())
				NextPoint = 0;

			sf::Vector2f Line1 = hull1->ToGlobal(hull1->Coords[Points1]);
			sf::Vector2f Line2 = hull1->ToGlobal(hull1->Coords[NextPoint]);
			sf::Vector2f Edge = Line2 - Line1;

			sf::Vector2f Axis = Util::VecNormal(Edge);

			sf::Vector2f Proj1 = ProjectShape(hull1, Axis);
			sf::Vector2f Proj2 = ProjectShape(hull2, Axis);
			
			float Interval = ProjectionOverlap(Proj1, Proj2);
			if (Interval < 0) //Not intersecting
			{
				return false;
			}
			else
			{
				Interval = std::abs(Interval);
				//std::cout << "overlap: " << Interval << "\n";
				if (Interval < fOverlap)
				{
					fOverlap = Interval;
					OverAxis = Axis;
					SmallEdge1 = Line1;
					SmallEdge2 = Line2;
					EdgeOwner = hull1;
				}
			}
		}

		for (int Points2=0; Points2 < PointCount2; Points2++)
		{
			int NextPoint = Points2 + 1;
			if (NextPoint == hull2->GetPointCount())
				NextPoint = 0;

			sf::Vector2f Line1 = hull2->ToGlobal(hull2->Coords[Points2]);
			sf::Vector2f Line2 = hull2->ToGlobal(hull2->Coords[NextPoint]);
			sf::Vector2f Edge = Line2 - Line1;

			sf::Vector2f Axis = Util::VecNormal(Edge);

			sf::Vector2f Proj1 = ProjectShape(hull1, Axis);
			sf::Vector2f Proj2 = ProjectShape(hull2, Axis);


			float Interval = ProjectionOverlap(Proj1, Proj2);
			if (Interval < 0) //Not intersecting
			{
				return false;
			}
			else
			{
				Interval = std::abs(Interval);
				if (Interval < fOverlap)
				{
					fOverlap = Interval;
					OverAxis = Axis;
					SmallEdge1 = Line1;
					SmallEdge2 = Line2;
					EdgeOwner = hull2;
				}
			}
		}
		sf::Vector2f MTV = Util::VecScale(OverAxis, fOverlap);

		sf::Vector2f d = hull1->GetPos() - hull2->GetPos();
		if (Util::VecDot(d, OverAxis) > 0)
			OverAxis = -OverAxis;

		if (!hull1->GetMoveable() && hull2->GetMoveable()) //Handle world shit
		{
			hull2->SetPos(hull2->GetPos() + Util::VecScale(OverAxis, fOverlap));
			hull2->GetOwner()->SetPos(hull2->GetPos() + Util::VecScale(OverAxis, fOverlap));
		}
		else if (hull1->GetMoveable() && !hull2->GetMoveable())
		{
			hull1->SetPos(hull1->GetPos() + Util::VecScale(OverAxis, -fOverlap));
			hull1->GetOwner()->SetPos(hull1->GetPos() + Util::VecScale(OverAxis, -fOverlap));
		}
		else 
		{
			float Total = hull1->GetMass() + hull2->GetMass();
			float H1Factor = hull2->GetMass() / Total;
			float H2Factor = hull1->GetMass() / Total;
			hull1->GetOwner()->SetPos(hull1->GetPos() + Util::VecScale(OverAxis, -fOverlap * H1Factor));
			hull1->SetPos(hull1->GetPos() + Util::VecScale(OverAxis, -fOverlap * H1Factor));
			hull2->GetOwner()->SetPos(hull2->GetPos() + Util::VecScale(OverAxis, fOverlap * H2Factor));
			hull2->SetPos(hull2->GetPos() + Util::VecScale(OverAxis, fOverlap * H2Factor));
		}

		float SmallestDist = 1000000;

		PhysicsObject* PointOwner;
		if (EdgeOwner == hull1)
			PointOwner = hull2;
		else
			PointOwner = hull1;

		for (int i=0; i < PointOwner->GetPointCount(); i++)
		{
			sf::Vector2f tPoint;
			tPoint = PointOwner->ToGlobal(PointOwner->Coords[i]);

			sf::Vector2f Dir = Util::Normalize(SmallEdge2-SmallEdge1);
			sf::Vector2f TL = SmallEdge1 + Util::VecScale(Dir , Util::VecDot(tPoint - SmallEdge1, Dir));

			float TestDist = Util::LengthSquared(tPoint - (SmallEdge1 + Util::VecScale(Dir , Util::VecDot(tPoint - SmallEdge1, Dir))));
			
			if (TestDist < SmallestDist)
			{
				SmallestDist = TestDist;
				CollideInfo[iCollideCount].Normal = -OverAxis;
				CollideInfo[iCollideCount].CollisionPoint = tPoint;
				CollideInfo[iCollideCount].Penetration = fOverlap;
				sf::Vector2f Vel1 = hull1->GetPointVelocity(tPoint);
				sf::Vector2f Vel2 = hull2->GetPointVelocity(tPoint);
				CollideInfo[iCollideCount].RelativeVelocity = Vel1 - Vel2;
				//iCollideCount++;
			}
		}

		return 1;
	}
	return 0;
}

sf::Vector2f PhysicsManager::Support(PhysicsObject* h1, PhysicsObject *h2, sf::Vector2f dir)
{
	return h1->FarthestPoint(dir) - h2->FarthestPoint(-dir);
}

sf::Vector2f PhysicsManager::TripleProduct(sf::Vector2f A, sf::Vector2f B, sf::Vector2f C)
{
	sf::Vector2f res;
	res = Util::VecScale(B, Util::VecDot(A,C)) - Util::VecScale(C, Util::VecDot(A,B));
	return res;
}

bool PhysicsManager::DoSimplex(sf::Vector2f &dir)
{
	sf::Vector2f A = Simplex.back();
	sf::Vector2f AO = -A;

	if (Simplex.size() == 2)
	{
		sf::Vector2f B = Simplex[0];
		sf::Vector2f AB = B - A;
		dir = TripleProduct(AB, AO, AB);
	}
	else if (Simplex.size() == 3)
	{
		sf::Vector2f B = Simplex[1];
		sf::Vector2f C = Simplex[0];
		sf::Vector2f AB = B - A;
		sf::Vector2f AC = C - A;

		sf::Vector2f ABPerp = TripleProduct(AC, AB, AB);
		sf::Vector2f ACPerp = TripleProduct(AB, AC, AC);

		if (Util::VecDot(ABPerp, AO) > 0)
		{
			Simplex.erase(Simplex.begin()); //Remove C
			dir = ABPerp;
		}
		else
		{
			if (Util::VecDot(ACPerp, AO) > 0)
			{
				Simplex.erase(Simplex.begin() + 1); //Remove B
				dir = ACPerp;
			}
			else
			{
				return true;
			}
		}
	}
	return false;
}

int PhysicsManager::CheckCollision(PhysicsObject* hull1, PhysicsObject* hull2, CollisionData *CollideInfo)
{
	Simplex.clear();
	sf::Vector2f SearchDir = sf::Vector2f(1,0);
	Simplex.push_back(Support(hull1,hull2, SearchDir));

	SearchDir = -SearchDir;

	while (true)
	{
		Simplex.push_back(Support(hull1,hull2, SearchDir));
		if (Util::VecDot(Simplex.back(), SearchDir) <= 0)
		{
			return 0;
		}
		else
		{
			if (DoSimplex(SearchDir))
			{
				std::cout << "Collide" << Util::Random(1,10) << "\n";
				return 0;
			}
		}
	}
}

void PhysicsManager::Tick()
{
	std::list<BaseEntity*>::iterator i;

	i = PhysicsEnts->mList.begin();
	while (i != PhysicsEnts->mList.end())
	{
		BaseMoveable *ent1 = dynamic_cast<BaseMoveable*>(*i);
		PhysicsObject* hull1 = ent1->GetPhysicsObject();
		std::list<BaseEntity*>::iterator i2;
		i2 = i;
		while (i2 != PhysicsEnts->mList.end())
		{
			BaseMoveable *ent2 = dynamic_cast<BaseMoveable*>(*i2);
			PhysicsObject* hull2 = ent2->GetPhysicsObject();
			if (ent1 != ent2 && ent1->GetPhysicsObject()->GetSolid() && ent2->GetPhysicsObject()->GetSolid() && ent1->ShouldCollide(ent2) && ent2->ShouldCollide(ent1))
			{
				int Collided;
				CollisionData CollideInfo[4];
				Collided = CheckCollision2(hull1, hull2, CollideInfo);
				//Collided = CheckCollision(ent1->GetPhysicsObject(), ent2->GetPhysicsObject(), CollideInfo);
				if (Collided > 0)
				{
					for (int i=0; i < Collided; i++)
					{
						CollideInfo[i].CollisionCount = Collided;
						//Only Call ent1 because it handles both impulses
						hull1->OnCollide(hull1, hull2, CollideInfo[i]);
						
						//Call the callbacks for the entities themselves
						ent1->OnCollide(ent1,ent2, CollideInfo[i]);
						ent2->OnCollide(ent2,ent1, CollideInfo[i]);
					}
				}
			}
			i2++;
		}
		i++;
	}
	i = PhysicsEnts->mList.begin();
	while (i != PhysicsEnts->mList.end())
	{
		BaseMoveable *ent1 = dynamic_cast<BaseMoveable*>(*i);
		ent1->GetPhysicsObject()->Integrate(GameGlobals::Instance()->FrameTime);
		i++;
	}
}